#! /usr/bin/env python
from dynamic_reconfigure.parameter_generator_catkin import *
from local_planner_limits import add_generic_localplanner_params
PACKAGE='collvoid_local_planner'

gen = ParameterGenerator()

add_generic_localplanner_params(gen)

gen.add("sim_time", double_t, 0, "The amount of time to roll trajectories out for in seconds", 1.7, 0)
gen.add("sim_granularity", double_t, 0, "The granularity with which to check for collisions along each trajectory in meters", 0.025, 0)
gen.add("angular_sim_granularity", double_t, 0, "The granularity with which to check for collisions for rotations in radians", 0.1, 0)

gen.add("path_distance_bias", double_t, 0, "The weight for the path distance part of the cost function", 32.0, 0.0)
gen.add("goal_distance_bias", double_t, 0, "The weight for the goal distance part of the cost function", 24.0, 0.0)

gen.add("goal_sq_dist", double_t, 0, "The sq_dist where heading criticts kicks in.", 2, 1.)


gen.add("collvoid_scale", double_t, 0, "The weight for the collvoid part of the cost function", 24.0, 0.0)
gen.add("occdist_scale", double_t, 0, "The weight for the obstacle distance part of the cost function", 0.01, 0.0)

gen.add("stop_time_buffer", double_t, 0, "The amount of time that the robot must stop before a collision in order for a trajectory to be considered valid in seconds", 0.2, 0)
gen.add("oscillation_reset_dist", double_t, 0, "The distance the robot must travel before oscillation flags are reset, in meters", 0.05, 0)
gen.add("oscillation_reset_angle", double_t, 0, "The angle the robot must turn before oscillation flags are reset, in radians", 0.2, 0)

gen.add("forward_point_distance", double_t, 0, "The distance from the center point of the robot to place an additional scoring point, in meters", 0.325)

gen.add("use_dwa", bool_t, 0, "Use dynamic window approach to constrain sampling velocities to small window.", True)
gen.add("use_dwa_scoring", bool_t, 0, "Use dynamic window approach to constrain sampling velocities to small window.", True)

gen.add("orca", bool_t, 0, "Use Orca instead of VOs", False)
gen.add("convex", bool_t, 0, "Use Footprint instead of Circumscribed radius", False)
gen.add("clearpath", bool_t, 0, "Use clearpath instead of sampling based", False)
gen.add("use_truncation", bool_t, 0, "Truncate VOs", True)
gen.add("num_samples", int_t, 0, "NUmber of Samples", 400, 1, 1000)

VO_enum = gen.enum([gen.const("VOs", int_t, 2, "Use velocity Obstacles"),
                    gen.const("RVOs", int_t, 1, "Use reciprocal velocity Obstacles"),
                    gen.const("HRVOs", int_t, 0, "Use hybrid reciprocal velocity Obstacles")], "Set the VO type")

gen.add("type_vo", int_t, 0, "Set the VO type", 0, 0, 2, edit_method=VO_enum)

#diff = gen.add_group("Settings for Diff drive")
gen.add("time_to_holo", double_t, 0, "In how much time I want to be on the ", 0.4, 0, 1)
gen.add("min_error_holo", double_t, 0, "How much error do I allow minimally", 0.01, 0, 1)
gen.add("max_error_holo", double_t, 0, "How much error do I allow maximally", 0.15, 0, 1)

gen.add("trunc_time", double_t, 0, "Truncation of Velocity Obstacles", 5.0, 0, 100)
gen.add("left_pref", double_t, 0, "Preference to avoid left", 0.1, -5.0, 5.0)

gen.add("restore_defaults", bool_t, 0, "Restore to the original configuration", False)

exit(gen.generate(PACKAGE, "collvoid_reconfigure_node", "Collvoid"))
